#include <iostream>
#include <thread>
#include <chrono>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <Utils.h>
using namespace std;
namespace xio=lyxutils::io;
namespace str=lyxutils::str_utils;

void printUsage(){
    cout<<"usage:visual_normal pc.txt"<<endl;
    cout<<"      pc.txt=>point cloud file,columns:x,y,z,nx,ny,nz"<<endl;
    cout<<"arguments:"<<endl;
    cout<<"  -h           print this help message"<<endl;
    cout<<"  -l=len       length of normal vector arrow(m),default is 2m"<<endl;
    cout<<"  -f=nx,ny,nz  indices of fields of normal vectors,default is -f=-3,-2,-1"<<endl;
    cout<<"               (-f=-3,-2,-1 means using last three columns as normal vectors)"<<endl;
    cout<<"  -s=size      set the point size as size(default is 4)"<<endl;
    cout<<"  -d=density   set normal display density(default is 5),display normal for every"<<endl;
    cout<<"               density'th point,this controls normal display density"<<endl;
}

int main(int argc, char**argv) {
    //显示点云法向量，点云x,y,z,nx,ny,nz
    xio::CLParser parser;
    parser.parse(argc,argv);
    vector<string> paras=parser.getParameters();
    if(paras.size()!=1 || parser.hasOption("h")){
        printUsage();
        return 1;
    }
    float len(2);
    try{
        len=parser.hasOption("l")?stof(parser.getOptionArg("l")):2;
    }catch(invalid_argument ia){
        cout<<"invalid argument:"<<ia.what()<<endl;
    }
    int nx(-3),ny(-2),nz(-1);
    try{
        vector<string> nxyz=parser.hasOption("f")?str::split(parser.getOptionArg("f"),",\t "):vector<string>();
        if(parser.hasOption("f")){
            if(nxyz.size()!=3)throw invalid_argument("argument of -f is not correct,usage=>`-f=nx,ny,nz` where nx,ny,nz are integers");
            nx=stoi(nxyz[0]);
            ny=stoi(nxyz[1]);
            nz=stoi(nxyz[2]);
        }
    }catch(invalid_argument ia){
        cout<<"invalid argument:"<<ia.what()<<endl;
        return 1;
    }
    int psize(4);
    try{
        psize=parser.hasOption("s")?stoi(parser.getOptionArg("s")):4;
    }catch(invalid_argument ia){
        cout<<"invalid argument:"<<ia.what()<<endl;
        return 1;
    }
    int density=5;
    try{
        density=parser.hasOption("d")?stoi(parser.getOptionArg("d")):5;
    }catch(invalid_argument ia){
        cout<<"invalid argument:"<<ia.what()<<endl;
        return 1;
    }
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::Normal>::Ptr normal(new pcl::PointCloud<pcl::Normal>);
    vector<vector<float> > fields;
    xio::read_csv_customize(argv[1],[&](const std::vector<float> &row){
        int cols=row.size();
        cloud->points.push_back({row[0],row[1],row[2]});
        normal->points.push_back({row[(nx+cols)%cols],row[(ny+cols)%cols],row[(nz+cols)%cols]});
    },",",std::cout);
    cout<<"total points:"<<cloud->size()<<",total number of normals:"<<normal->size()<<endl;
    pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("cloud viewer"));
    //set background
    // viewer->setBackgroundColor(5,55,10);
    //set color of point cloud
    // pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(0,255,0);
    //add coordinate system
    // viewer->addCoordinateSystem(1.0);
    viewer->addPointCloud<pcl::PointXYZ>(cloud,"cloud1",0);
    cout<<"set normal arrow length="<<len<<endl;
    cout<<"use field#:"<<nx<<","<<ny<<","<<nz<<" as (nx,ny,nz)"<<endl;
    viewer->addPointCloudNormals<pcl::PointXYZ,pcl::Normal>(cloud,normal,density,len,"normals");
    //set point size
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, psize,"cloud1",0);
    while(!viewer->wasStopped()){
        viewer->spinOnce();
        // std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }
}
